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ABSTRACT 


The Naval Postgraduate School (NPS) has been developing real-time 3D visual simula- 
tors on inexpensive commercially available graphics workstations. The effort to develop å 
3D visual simulation system, NPSNET, is an exploration and experimentation virtual 
worlds. Virtual world system have many goals including military training. This work is part 
of NPSNET. The current NPSNET system is kinematically based. The objectives of this 
work are motion dynamics and behavioral motion control for autonomous vehicles. Motion 
control is difficult when using dynamics due to the internal and external forces, however it 
enhances the realism as well as motion accuracy. We develop motion dynamics and behav- 
ioral control for ground-based vehicles. Motion dynamics and behavioral motion are an es- 


sential part of NPSNET for realistic battlefield simulation. 


111 


II. 


Ш. 


В. NPSNET 


D. THESIS ORGANIZATION 


A. INTRODUCTION 
B. DYNAMICS OF RIGID BODY MOTION 
1. 


e NEWTON-EULER EQUATIONS 
1 
2 
VEHICLE MODELING AND CONSTRAINTS 
A. VEHICLE MODELING 
IS 
2: 
B. VEHICLE CONSTRAINTS 
1. 


Tracked Vehicle 
Wheeled Vehicle 


Engine And Brake Constraints 


TABLE OF CONTENTS 


Center OF Mass Е eee 
Linear Momentum СЕЕ 
Angular Momentum tae E me es 


ЕКЕ.‏ 6 3ر116 


Linear And Angular Acceleration ....................... 


Inte gration SJ OT NO a a 


A. MOTIVATION ҒОК КЕЅЕАКСН ................................ 


© © ©© 6ه ©6©6 56 ه25 6ه .هه هه © 


© ©6ه© © © ©6© ©6ه© 66 66 هه ه ه‎ ©6© ое ое ©6© © © © 6© ©6© е ое оо е © © © © © © © © © © © © © © © © © © © © © © © © © 
КК ААА ИАА ААВ] 
20000000000000000000 


90000000000000000 


29000000000000000000000 
ФФФФФФееееееееееееееееееееееееееееееееееееееееее 

...9.9.9 о.. 6© .њ. еее ое ее еее © © 
.9 е 99 ао 44 њќЧМ6о оо оо оо оо соо е 

"99.9... еее е оо еве 


ФФОФЖеееееееегеееееееееееееееее 





VUULEY ANOA LIBRARY 
NAVAL POSTGRADUATE SCHOOL 
MONTEREY CA 93943-5101 


D. ЕШШ CL RSG 17 

3. GEGEN 18 

IV. ЕВ АТО ال‎ 20 
А. ЕРИРЛУЮОЛИТОЫ СОМИКОІ, ......................................... 20 


В. IMPACT OF ENVIRONMENT AND VEHICLE MOTION .......21 


1. Serena Rance And Detection eerie.. esed 21 

2 Verse ONNAN ra AAA ACO 24 

3; AO аба еее eene nno 25 

с FLOSS إن‎ ВБЕПАУ КАЛ МОТО ........................шшә. е... 26 

М. HETE MED LATION ISSUES аа ннн е noo oett 28 
A. SS STE EE cH Em 28 

B. ЛЕБЕДЕВА КИЕВ оодара анана 29 

C Е ОО ОО ВО. пони. 31 

1. ВЕ Пао ВО синь 31 

2. vehicle Speed And Direction ас Se ъваа 31 

Г). E О و‎ ina 36 

Е; USER INTERFACES AND PERFORMANGCE............................ 37 

Ne ONO SIONS AND FURTHER WORK „........... нон 20 66 01106860 38 
нее. 39 
КЕЛЕКЕ Ө ЕТИКЕ К. у уу.........:...............................— 41 
ное нии 56 
Ше ОВАКВУ РОН 64 
И ЕВ ВОМ н 66 


FIGURE 2.1 
FIGURE 3.1 
FIGURE 3.2 
FIGURE 3.3 
FIGURE 3.4 
FIGURE 3.5 
FIGURE 4.1 
FIGURE 4.2 
FIGURE 4.3 
FIGURE 4.4 
FIGURE 4.5 
FIGURE 5.1 
FIGURE 5.2 
FIGURE 5.3 
FIGURE 5.4 
FIGURE 5.5 
FIGURE 5.6 
FIGURE 5.7 


LIST OF FIGURES 


Cartesian Coordinate System And Momentum Of Particle .............. 8 
Reference Frame 8 12 
Tracked Vehicle Acted On By Тууо Еогсе$ ..................................... 13 
Two Dgree-Of-Freedom Wheeled Vehicle Model ......................... 15 
Orientation Of Wheeled Vehicle By Two Positions........................ 16 
Gravity And Mass On An Inclined $игїасе..................................... 19 
Searching Area ооо с ОО 22 21 
Algonthm For Obstacle Detection A 77000 23 
Algorithm For Avoidance Direction лт хо 2 лии 25 
Obstacle Avo1dånee Pam eec NE TA 26 
200000 ETA 27 
System НОО. ee 28 
Ground Vendee Sire me c срна 30 
Algorithm For Integration Of All Forces о а 32 
Algorithm For Vehicle Acceleration And Velocity ....................... 33 
Algorithm For Vehicle Direction Computation ............................. 34 
Motion ConrolFIm SR NT 35 
Algorithm For Obstacle Nodne SP SN 36 


vi 


ACKNOWLEDGEMENTS 


I would like to thank Korean government and Korean Army. I have come to the Naval 
Postgraduate School to study computer science for two years with their grant. Also, my sin- 
cere thanks to the U.S Department of Navy. I always thank my loving family for their en- 


couragement from the beginning. 


This work would not have been done without useful help from many people. I would like 
to thank Dr. Michael Zyda for his guidance from the first to the end of this work and Mr. 
David Pratt for his aid during the entire implementation. Lastly, I also thank Dr. Sehung 


Kwak for useful discussions concerning related work and the motivation of this research. 


vli 





I. INTRODUCTION 


A. MOTIVATION FOR RESEARCH 


Recently simulation technology has been adapted in many ways. The flight simulator 
is used for training pilots and astronauts to safely control complex, expensive vehicles 
through simulated environments [Deyo, 89]. The capability of graphics workstations drops 
the relative cost of simulation, hence the technology has been moving gradually toward 
lower cost simulation environments through which the operator is also able to control his 


own viewpoint or motion. 


An advanced ground vehicle simulator is made up of a few, expensive subsystems that 
combine to create a complete virtual environment for the operator. Interactive real-time 
ground vehicle simulation demands scene realism and accuracy of motion. Because of the 
rapid development of computer technology, current computer hardware and software make 
it possible to synthesize realistic images. However, the real-time physical simulation of 
motion still remains a difficult task. It is now possible to use vehicle dynamics for 


simulating real world motion on a general purpose workstation. 


Dynamics can produce complex motion with minimal user inputs, but it 1s 
problematic in the difficulty of motion control and it is computationally expensive. A more 
automatic method of controlling motion can improve the control problem. Behavioral 
simulation is a means of automatic motion control according to certain rules. We can use 
behavioral control] for autonomous moving vehicles in the simulation system. The research 
of motion dynamics and autonomous motion planning of a vehicle simulator on a general 
purpose workstation is a key component for the construction of a fully interactive virtual 


world. 


B. NPSNET 


The Defense Advanced Research Projects Agency (DARPA) has been developing a 
simulation system entitled Simulation Networking (SIMNET) [Thorpe, 87]. SIMNET is an 
interactive battlefield simulation system designed for military training. SIMNET nodes 
require visual displays and specific hardware of a particular vehicle. The effort to develop 
alow-cost SIMNET type system based on commercially available graphic workstations has 
been an ongoing project at the Naval Postgraduate School (NPS). The system, NPSNET, is 
a real-time, 3D visual simulation system capable of displaying vehicle movement over the 
ground or in the air. NPSNET runs on Silicon Graphics workstations attached to a local area 


Ethernet and uses SIMNET databases and networking formats [Zyda, et al., 92]. 


In NPSNET, up to 500 of the vehicles can be driving in the world at one time. The user 
can select any one of the active vehicles via mouse selection and control it with a six degree 
of freedom SpaceBall or button and dialbox. The pick button on the SpaceBall fires a round 
from the driven vehicle.Vehicles can be controlled by a prewritten script, driven 
interactively from other workstations, or autonomously via computer control. Displays 
show on-ground cultural features such as roads, buildings, and soil types. These vehicles 
move around in the virtual environment that is based on the terrain at Fort Hunter-Liggett, 


California. 


Depending upon the model of IRIS being used, the user can select to use texturing, 
and environmental effects such as fog and haze. A two dimensional (2D) map can be 
displayed that shows the position and tracking of all the players. This map displays the 
direction and viewing triangle of the driven vehicle as well as the position and movement 
of the remaining vehicles. The statistics and data concerning the driven vehicle (speed, 


pitch, roll, and so on) are displayed in an information window at the top of the screen. 


C. OBJECTIVES OF THIS WORK 


The current NPSNET system is kinematically-based. The motion description, from 
user-input through actual frame rendering, consists of positions specified over time. The 
goal of this work is to add motion dynamics to NPSNET. The aerodynamics model is being 
developed concurrently for air vehicles [Cooke, 92]. This work concentrates on the 
essential system issues for the implementation of virtual environments for real-time ground 


based vehicle simulation. 


1. Dynamics 


Dynamics deals with the motion of bodies under the action of forces. Motion 
occurs in the physical world as result of forces acting on objects. To achieve a degree of 
realism in motion control, the motion of objects must be simulated by the physical 
principles of dynamics. This work focuses on theoretical and numerical aspects of the 


implementation of a dynamic motion simulation system. 


2. Behavioral Control 


The search for more automatic methods of controlling motion is a major area in 
computer graphics. The problem of motion planning of autonomous vehicles consists of 
selecting the geometric path and vehicle speeds so as to avoid obstacles and it should 
minimize computationally expensive functions to keep the frame rate at an acceptable 
level. Motion is planned at a task-level and computed using physical laws. Inverse 
dynamics is the essence of the autonomous motion control. In higher level motion control, 
behavioral motion in particular, evaluates the state of the environment and generates 


motion according to certain rules of behavior [Wilhelms, 90]. 


3. Vehicle Parameterization 


The vehicles used in NPSNET are military vehicles such as tanks, APCs and 


trucks. To get the realism and satisfy the simulation system users, the vehicles’ motion 


should match the real vehicle specification. These specifications are defined in the data 


structure of the vehicles and incorporated with the dynamics equations. 


D. THESIS ORGANIZATION 


Chapter II covers basic dynamics in the vehicle simulation. Derived dynamics 
equations are defined and an integration method is presented. Chapter III provides vehicle 
modeling and motion control issues including dynamics as well as vehicle constraints. 
Chapter IV presents the high-level motion control issues, especially behavioral motion with 
inverse dynamics. Chapter V provides implementation details of the dynamic motion 
control and data structures into the simulator. Chapter VI covers conclusions and 


suggestions for further work in this area. 


II. DYNAMICS 


A. INTRODUCTION 


This chapter presents the rigid body dynamics and coordinate system that are used in 
the simulation. A rigid body can be described as a fixed and unchanging extended mass. 
The dynamics of general plane motion of a rigid body combines translation and rotation. In 
the vehicle dynamics analysis, vehicles are treated as extended rigid bodies, and 


information concerning their mass, centers of mass, and mass distribution is needed. 


Previously, the dynamics equations of a vehicle model are derived [Meriam et.al., Seil 
Motion is produced by a simplified simulation of dynamics, that describes the linear and 
angular accelerations of a rigid body in terms of the forces acting on them. 

If a number of forces are acting on the body, their total translational effect can be 
found by merely summing them. The center of mass of the body will move translationally 
as if it were a particle mass influenced by one net force. The external forces acting on the 
vehicle consist of the frictional forces between the vehicle and ground, the normal forces, 
and the gravity force. A torque is similar to a force, except that it causes a rotational motion 
about a particular axis. Torques can be represented as 3D vectors describing their 


components about an x, y, and z-axis. 


B. DYNAMICS OF RIGID BODY MOTION 


The body coordinate system is used in the simulation. Since a rigid body is free to 
rotate and translate, it is possible to describe the placement of a rigid body in world space 
in terms of the location of the body's center of mass and the body's rotational orientation 


in world space. 


l The mathematical dynamics equations we cover below are taken from this reference and 
then implemented with vehicle constraints. 


The basis vectors of this coordinate system are usually the principal axis of the object. 
For body i, the center of mass is represented by the vector c(t), and its orientation is 
represented by the 3x3 matrix R. The terms that are used in this chapter can be found in 


the Table 2.1. 


Inertia Tensor Matrix 
force 

Force due to gravity 
External applied force 
Torque 

Center of Mass 
Position Vector 
Orientation Matrix 
Linear velocity 
Linear acceleration 
Linear momentum 
Angular velocity 
Angular acceleration 
Angular momentum 


mass 


Time step between samples 


Moments of inertia 


Products of inertia 





Table 2.1. Dynamics Terms 


1. Center of Mass 


A representative particle of mass, m;, 1S located by its position vector, r;, of the 


reference axis. The center of mass of the system of particles is located by the position 


vector, 7, which, from the definition of the mass center as covered in Statics, is given by 


т? = mr, (Ea 1) 

where the total mass M = £m, [Meriam et. al., 86]. 
At time f, the center of mass is location in world space given by the weighted sum 

Yim;r,(t) zm ,r, (t) 
gs таи (Бара?) 


1 
If using the body coordinate system, the center of mass in body coordinate is at the origin. 


This means that the body space is 


Emrit) 2 (Eq. 2.3) 
ИЯ 4-2 


which means that Em,r,, - O as well. 
In the motion simulation, the body coordinate system is used for the following 
relations. Let c(t) be the location of the center of mass in world space. Since the center of 


the mass is located at c(t), the location of the Гу mass point in world space is 


r, (t) = R (t)r; +c (t). At time t, since R(t) Y m;rg, - R(t)O = O, the Equation 2.2 is 


2mr; (t) im, (R (t)ro;j + € (£) ) 5 


im, 24 
M КН RE CE (Ед. 2.4) 


Sim, (7; се) УтВатуне(д -е(д) = R() 9 mirgi = O (ЕД 25) 
These relations enable us to consider linear translation separately from angular 
orientation. Thus it is possible to combine a rotation followed by a translation without 


changing the size or shape of the body. This can simplify the description of the laws of 


motion. 


2. Linear Momentum 


The linear momentum p of a particle with mass m and velocity v is defined as 
DBD mU (Eq. 2.6) 
Since the derivative of velocity is acceleration a, Newton’s law of motion for a particle 


mass can be expressed as 


E сал mc 
f= ma = na == (тр) s (га 7) 
This law says that the rate of change of the momentum p of a particle is equal to the force 
f acting on the particle. Since f = p for a single particle, itis obvious to expect that a similar 


law holds for rigid bodies, comprised of collections of particles. 


3. Angular Momentum 


There should be a linear relation between the body’s angular momentum and its 
rotational velocity. The angular momentum depends on the choice of origin of coordinates. 
The angular momentum l;(1) of the i,, particle of the rigid body is 

I(t) = (7; (t) —¢(t)) xm, (7, (t) —¢ (t)) (Eq. 2.8) 
Thus, /;(t) is the cross product of the i,, particle’s displacement from the center of mass and 
the momentum of the particle with respect to the center of mass. The total angular 


momentum L(t) of the body is simply 


Г = Ува) = ORD) Sire (Œq. 29) 


r position 


Coordinate 





Figure 2.1 Cartesian Coordinate System And Momentum Of Particle 


4. Inertia Tensor 


The inertia tensor of a body is a 3 x 3 matrix that describes the distribution of mass 
in the body. For symmetrical bodies, there are simple ways of calculating the moments of 
inertia. For a box centered at the origin with width c in x, b in y and a in z, multiplying the 


density by the volume gives the mass. In this case, the moment of inertia around the origin 


of x-axis 1S 
1, = ут (a? +b?) (Eq. 2.10) 
Similarly for y and z-axis, I, = im (а + с“) and I, = т (b* +c*) . The off-diagonal 
terms, such as / xy are 
т Р ео 
= ЗЕ сор (ху) ахауаг = J2. [2. хуахауаг =0 0200211 
m 9 yg mA on 


and similarly for the others because the integrals are all symmetric. Thus for the 


symmetrical block with width a in x, b in y and c in z, the inertia tensor matrix is 


M bac’ 0 0 | 
Doody = 12| 0 ос о (Eq. 2.12) 
0 0 at b^ 


[Baraff, 91]. 
С. NEWTON-EULER EQUATIONS 


1. Linear and Angular Acceleration 


Two parts of Newton-Euler equations are the translational motion of its center of 
mass and rotational motion about the center of mass. The linear momentum and angular 
momentum are used in the simulation for the linear velocity and angular orientation each. 

By Newton's law, the linear accelerations for x and z axis of the center of mass 
are derived from Equation 2.7. If c is the vehicle’s center of mass position and m is total 


mass, the equation of motion for linear acceleration a is second derivative of position c. 


f. uma mas ТОЕЛ) 

where a is in meter/second*. 
The torque, t, differs from a force in that the torque on a particle depends on the 
location of the particle. The torque acts on the particle, i, is t = (r;(t) -c(t)) x F. By the 


Equation 2.11, the products of inertia are zero. So, the rotational equations for motion about 


the center of mass are, 


т = О ОС (Eq. 2.14) 
t, = 1,0,+ (,- 12) 0,0, (Ва 21 
dE Пон Sole (Eq. 2.16) 


where the vectors are expressed in the principal axis frame of the vehicle. In the simulation, 


o is in radians/second and o in radians/second? [Wilhelms, 87]. 
The dynamics equations are solved for new angular and linear accelerations. 
These accelerations must then be integrated to find new velocities and integrated again to 


find new positions. 


2. Integration 
The Euler method uses six equations shown in Equation 2.13, 2.14, 2.15, 2.16. 
Because of real-time limitations, methods that require only a single updated evaluation of 


f for each time step are currently used. The Euler method is used for the integration. 


Ug, = V, +a Òt пещ 2 №. 
бы = 0,+ O,At + 0.50, At” (Ea. 2.18) 
Св = C, + U,At + 0.Ба Ле (Еа. 2,19) 


where 9 and c are the orientation and position at time t and t At. 
The Euler method has two advantages. It is simple to program and efficient. The 
velocity is calculated once in a time step. Also it is efficient. This assumes acceleration is 


constant over the time period. The disadvantage of the method is the assumption that the 


10 


velocity 1s varied slowly in a time step. The inaccuracy problem, discontinuity between the 
Current approximation speed and the next approximation speed, occurs when the time 
period is large or accelerations are changing rapidly. However with reasonably small time 
steps, the Euler method can produce appropriate result without too much trouble arising 
[Wilhelms, 87]. 

In the simulation, the Euler method is used for this reason and the result is reasonable 
for ground based vehicles. The other method is investigated and implemented for air 


vehicles and both methods are compared in [Cooke, 92]. 
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ПІ. VEHICLE MODELING AND CONSTRAINTS 


A. VEHICLE MODELING 


In NPSNET, there are two types of ground vehicles: tracked and wheeled. All the 
vehicles can be approximated as rectangular blocks with the same mass density. The center 
of mass is assumed to be the center of the block. This assumption can reduce the dynamics 
calculation without the consideration of the vehicle types. The reference and path 
coordinates are shown at Figure 3.1. 


Body coordinates in the simulation for the vehicle description are subject to 


holonomic constraints”. The direction of the wheeled vehicle is computed by its front wheel 
angle, however the tracked vehicle’s direction is changed by the difference between the left 
and right track force. The motion of a vehicle is also limited by its constraints. In 
translational motion, the maximum acceleration 1s not always positive, nor 1s the maximum 
deceleration negative. This depends on the slope of the path. The engine forces of vehicles 
moving on bumpy terrain need to be carefully selected in order to maintain continuous 


contact with the ground as well as maintaining speed [Shiller et. al., 91]. 


Z 


Reference Frame 





Figure 3.1 Reference Frame And Path Coordinates 


Holonomic constraints mean P(c;) = 0 (constraints in precisely this functional form 
which can be used to eliminate dependent coordinates. c is body coordinates). Vehicles are 
assumed to be block structures. 
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1. Tracked Vehicle 


Result Motion 


Ш 


Result Motion 


J Applied Forces / 


Translation Rotation 





Figure 3.2 Tracked Vehicle Acted On By Two Forces 


Tracked vehicles have two internal forces to move the vehicle, the left track force 
and the right track force. The two tracks of the tracked vehicle are assumed to be parallel. 
The distances from the center of mass to each track are the same. 


The internal forces, f; and f,, act on the vehicle at points other than the center of 


mass and cause vehicle rotation as well as translation. In Figure 3.2, if left and right forces 
are applied to the vehicle with the same direction and quantity, they produce translational 
motion without torque. However, two equal opposite forces cause a torque without a linear 
acceleration. The moments of inertia around the center of mass can be found from Equation 
20 

The translational factor, velocity, is computed by Equation 2.17 and the vehicle 
rotation about the center of mass is presented in Equation 2.15. From those equations, each 


axis’ velocities are 
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U, = UV C050 0 ETD 
The x and z coordinates of new vehicle position can be obtained by Equation 2.19. 
Rotational velocity is merely calculated by the sum of two internal forces, 
f, = f,+f,. The opposite direction force has negative value. Torque is computed by this опе 


force and angular orientation is obtained by Equation 2.18. 


2. Wheeled Vehicle 


The four-wheel vehicles with two fixed axles, such as the M-35 truck, are driven 
by fixed rear wheels and steered by the front wheels. Since the vehicle is simplified as a 
block structure, the motion of four wheels are neglected and the direction is computed as a 
parameter of the front wheels. The moment of inertia of the wheels is neglected and the 
wheels are making contact with the ground. 

The wheeled vehicle model is similar to the tracked vehicle in translational 


motion. But the steering angle and the vehicle orientation of the wheeled vehicle can be 


derived by non-holonomic constraints? [Shiller et. al., 91]. Choosing a center of mass as the 
guiding point, and assuming no sliding, makes the vehicle's orientation non-holonomic 
since only incremental changes for vehicle orientation and steering angle are possible. 
The four tires are not treated as individual bodies, but those are considered for the 
orientation of the vehicle. In Figure 3.1, the r vector is normal to the surface, 1 15 tangent to 
the path, and g is normal to both. A unit vector, n, in the rq plane is defined in the direction 
of the center of path curvature. At a velocity, v, the front wheel angle moves the vehicle 
along the path. In Figure 3.3, dis a distance from front axle to Center of Mass and the same 
for a distance to the rear, since we assume that all vehicles’ center of masses are located at 


the center of vehicles. 


3 Non-holonomic constraints mean non-integrable relations involving the coordinates. 
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The direction of the wheeled vehicle is 
2d 
4p? - d* 


where d 1s half the distance between the axles, and p is the distance to the Center of Turn 





tana - 


(qoo) 


from the mass center. The x and z-axis of the vehicle, that are parallel to the terrain, are 
determined based on the orientation and the steering angle at the previous point [Shiller et. 


al., 91]. 





Center of Turn 


Figure 3.3 Two Degree-Of-Freeedom Wheeled Vehicle Model 

The angle 0 is a function of the current vehicle position p and the desired direction 
of motion t. Since pis the result of the previous motions, it can be solved for the incremental 
changes in 0 [Shiller et. al., 91]. 

Figure 3.4 shows the vehicle at two positions: before and after an incremental 
move of As along the path from some point i. The mass center of vehicle at i+1 is located 
at the end of the vector Ast, from the mass center of the vehicle ati. During movement, the 
rear wheel moves along the p; direction to a distance a from the mass center of vehicle at i. 
The new location of mass center determines the orientation of the vehicle. Then p; can be 


defined as 


15 


p, = 0504, - віп0 4, (Equo) 
The vehicle's new orientation at i* 1 15 


AS 


a 
gU RE (Ед. 3.4) 


Dii 
where steering angle is 
a = -Азсо5б, + а? - Asa sinê, (Ед. 3.5) 
Consequently, the angle 0, ,, is 
6 — аб fi+ )ر‎ (Ед. 3.6) 


The angle 0, , , is used for the next computation as Ө. 


Vehicle at I 





Vehicle at i+1 


Figure 3.4 Orientation Of Wheeled Vehicle By Two Positions 


B. VEHICLE CONSTRAINTS 


Several constraints between the vehicle and the environment are considered to ensure 
vehicle dynamic stability along the path. Various forces such as friction, gravity, engine 
and brake torgue are included in the simulation. 

The external forces acting on the vehicle consist of the fnction forces between the 
vehicle and ground, the normal forces and the gravity force. The total external forces are 


the sum of those forces. 
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1. Engine And Brake Constraints 


Engine torque produces frictional force and pushes the vehicle either forward or 
backward. A positive engine torque produces a force in the direction of motion, while a 
negative torque, or a braking force, produces a force in the opposite direction. This force is 
bounded by the maximum equivalence engine force F ma and maximum braking force F pin 

а em Ват, 

The force f, 1s the internal force of the vehicle. The limitation of the forces is 
validated by the real vehicle specification. The rpm value of the engine is changed by 
internal force not the vehicle speed. The engine torque constraint limits only the 


acceleration. 


2. Friction 


Friction is a complicated phenomenon and modeling it exactly is challenging. An 
approximate treatment is good enough for most engineering purposes, and suffices to 
produce visually reasonable effects on motion [Wilhelms, 88]. 

The tracks or wheels are making contact with the ground. At each contact point, 
there are three unknown forces: two friction and one normal force. Friction forces act to 
oppose tangential motion along the surface and are dependent upon the normal force 
pressing into the ground. 

Basically, each surface is assigned a stick angle. The intuitive meaning of this 
angle is that an object will sit motionless on that surface as long as the tilt relative to gravity 
is less than the stick angle. As soon as the tilt exceeds that limit, the vehicle starts to slide. 


The total friction force, Fyj,, tangent to the xz plane, is represented in path 


coordinates as 
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Frie = fit Да (Ед. 3.8) 
where f, and f, are the components tangent and normal to the path, respectively. The 
equation of motion of the vehicle can be written in terms of the tangential speed v and the 
tangential acceleration a. 
ft + fa —mgk = трпи“ 4 тва“ (Ед. 3.9) 
where & 1s path curvature, g 1s gravitational acceleration and m is the vehicle mass. The 
reaction force in the r direction when the vehicle moves along the bumpy terrain is 


neglected. The projection of the external forces in the 1, g, and r directions is obtained by 


dot multiplying both sides of Equation 3.6 with the vector t, q and r. 


f, = mgk,+ma Eg За) 


f, = mgk,+ mkn v" (Eq. 3.11) 


In world coordinates, the friction forces in the xz coordinates are 


F pic x = 110050 -f ѕіп Ө (Ед. ЭЯ) 
Frric-z = hsin0+f,cos8 (Еа. 3.19) 


The friction force required by vehicle motion is XF7,ic-1+ Frric-z> that is divided 
into dynamic friction and static friction. Dynamic friction 1s applied to moving vehicles and 
static friction 1s applied to unmoving vehicles in the simulation. 

3. Gravity 
The effect of gravity is easily calculated given the gravitational acceleration 


(about 9.81 m/sec? on the earth’s surface). For the center of mass on an inclined surface, 


the applied gravity force is 
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Его “ (0,-9.81,0) m (Eq. 3.14) 


If the inclination angle is 6, then A = (—-sin®,cos0,0) . That is the gravity force that applied 


to the vehicle is 


Forv = Å *fyry = mgeos0 (Bq, 3.15) 


gru 
If the gravity force F 4,4, exceeds the friction force Fr. of the unmoving vehicle on an 
inclined surface, the vehicle should slide either forward or backward. 
The torque due to this force acting in the body fixed coordinate frame is 


(Eq. 3.16) 


С JUR 
while c is the center of mass and т 15 angular torque. This torque acts on the pitching and 
rolling of the vehicle. The reaction force by this torque is neglected in the simulation. 
Maximum pitching and rolling angles are determined by the slope of the terrain. Hence 


during the simulation, vehicles continuously contact the ground. In Figure 3.5, 6, is 


pitching angle and 0 is rolling angle. 


-mg 


e. 


ж 
کے‎ ١ 





Figure 3.5 Gravity And Mass On An Inclined Surface 
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IV. BEHAVIORAL CONTROL 


A. ADAPTIVE MOTION CONTROL 


The problem of motion planning of autonomous vehicles consists of selecting the 
geometric path and keeping the appropriate vehicle speed so as to avoid obstacles. Adaptive 
motion control means that the environment has an impact on the vehicle motion and vice 
versa. Information about the environment and the vehicle such as location and orientation 
of objects and vehicles must be available during the control process. Adaptive motion 
makes possible goal-directed and constrained behavior, since it allows the user to describe 
movement in terms of relations among objects and vehicles. The user only specifies the 
broad outline of the motion, then the simulation system fills in the details [Zeltzer et. al., 
89]. 

Behavioral control is high-level control, however the motion is achieved by 
dynamics. It evaluates the state of the environment and generates motion according to 
certain rules of behavior [Wilhelms, 90]. In the simulation, obstacle avoidance is a basic 
rule of behavioral motion. Obstacle avoidance is a simple model of adaptive motion control 
and it is implemented for the autonomous vehicles in the simulation. The goals for 
behavioral control are to develop effective techniques for planning and following routes 
based on object information. 

In NPSNET, the object locations and terrain elevations are maintained in a text format 
file. Sensing detects the characteristics of obstacles in the environment and passes this 
information to the vehicles and the motion control routine. Sensors have a fixed position 
and orientation on the body. To save memory and improve the frame rate during the 
execution, the scope of obstacle detection is limited. All fixed objects are attached to the 
terrain, thus only the geometric data and object data in the field of view are used 1n obstacle 


detection. 
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B. IMPACT OF ENVIRONMENT AND VEHICLE MOTION 


1. Searching Range And Detection 

Obstacle avoidance motion is defined in two dimensions since all objects are 
ground-based. The information available on objects is their center coordinates and radius. 
We develop techniques necessary to detect obstacles with minimum delay between sensing 
and acting. During the execution, the information concerning the objects 15 maintained as 
an array. Only the objects in the field of view are kept in the array. There are more than 
40,000 natural and man-made objects in the Fort Hunter-Liggett terrain database used in 
NPSNET. Distance between an object and a vehicle can be obtained with a two 


dimensional computation. 


The field of view, as the angle in radians, about the present object heading is more than 
enough. The searching angle is narrow than the field of view. The searching angle 0,, 0, 1s 
10 degrees. This angle gives the vehicle with speed 60 km/h approximately 12.9 meters 


path corridor on the terrain. 


Left Right 
Search Triangle 


Search Triangle 





Figure 4.1 Searching Area 
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All objects are not displayed at one time. The objects which are located in the 
current field of view, are shown and those are kept in the avoidance object array. The range 
distance d, is determined by the speed of the vehicle. 
sored (Eq 4.1) 


d=d +d 


minimum 
where Gniminum 1S 20 meters and dypeeg is the vehicle speed in meters/second. 


Our search heuristic is left triangle first. Only one object can be detected at one 
time. The vehicle can change its path to avoid an object and then the vehicle starts the next 


search. 


p» 





Determine Range Distance (); 
Left search Angle = Vehicle Direction - HFOS; 
Right Search Angle = Vehicle Direction + HFOS; 












/* Sensing coordinates in x,z coordinates */ 


Sensing position = Current Vehicle Coordinates; 
























/* Determine left and right search area */ 
Determine Boundary Left Search Area (); 
Determine Boundary Right Search Area (); 





/* Looking for any obstacles in this area */ 
Left Search (); 
Right Search (); 


if(LEFT && RIGHT) /* Detect obstacles in left and right area */ 
/* If the obstacles are the same one */ 
if (Left Object == Right Object) 
DETECTION = LEFT: 
else /* If the obstacles are different one */ 
/* Measure the distance between the vehicle to the obstacles */ 
Left Distance = Distance In 2D(Sensing Position, Left Object); 
Right Distance = Distance In 2D(Sensing Position, Right Object); 


if (Left Distance >= Right Distance) 
Detection = RIGHT; 
else /* Left obstacle is nearer than right one */ 
Detection = LEFT; 
else 


if (Left) 

Detection = LEFT; /* Obstacle in left area */ 
else if (Right) 

Detection = RIGHT; !“ Obstacle in right area */ 
else 


Detection = FALSE; /* No avoidable obstacle */ 


Figure 4.2 Algorithm For Obstacle Detection 
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2. Inverse Dynamics 


The inverse dynamics problem of a moving vehicle consists of solving for vehicle 
orientation and translation. Translation and orientation of the vehicle can be derived from 
dynamics equations presented in Chapters II and III. 

The translational inverse dynamics problem is defined as an autocruise control in 
the simulation. The vehicle velocity is fixed when autocruise mode is selected. To get an 
appropriate engine force, we need the information of variance of external forces. The 
engine force must change due to the change of external forces. The summation of new 
internal forces of the vehicle and total external forces remains constant through the 
computation. Autocruise speed is computed by this constant force, hence the vehicle 
velocity is kept fixed. Autocruise speed control helps interactive user control during driving 
too. 

The direction of the wheeled vehicle is presented in Chapter III. Referring to 
Figure 3.4, the new locations of the mass center and the rear axle define the new vehicle 
orientation [Shiller et. al., 91]. From Equation 3.4, it is possible to solve the steering angle 
o. that moves the mass center along path in terms of current vehicle’s direction 0. The next 
steering angle o is obtained from the Equation 3.2 and Figure 3.3. 

a = atan (2tan6) (Eq. 4.2) 

For the tracked vehicle, total engine force is derived from the autocruise control 
routine. Then the left and right forces of the vehicle are changed until the vehicle’s new 
orientation matches the desired orientation. Figure 4.3 shows the algorithm for inverse 


dynamics of the vehicle direction problem. 
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/* For the tracked vehicle */ 
if (Detect Object) ( 
Compute Desired Orientation(); 
do while (New Orientation != Desired Orientation) ( 
if (Object In Left Search Tri) 
Left Track Force = Left Track Force - Delta; 
Right Track Force = Right Track Force + Delta; 
else /* Object in right search tri */ 
Left Track Force = Left Track Force+ Delta; 
Right Track Force = Right Track Force- Delta; 
Compute New Orientation(); 


/* For the wheeled vehicle */ 
if (Detect Object) [ 


Compute Desired Orientation(); 


do while (New Orientation !2 Desired Orientation) ( 


Compute New Orientation(); /* Equation 4.1 */ 





Figure 4.3 Algorithm For Orientation And Desired Path 


3. Path Determination 


The desired path of the vehicle 1s simple. The behaviors are grouped into two 
activities for the set of path determination. The first activity 1s to travel when the vehicle 1s 
in a clear area, and the second is to control the vehicle when obstacles are present. When 


the vehicle detects an object in the left search triangle, 7), it changes its direction to the 


right. The other case, when it detects any object in the right search triangle, it changes its 
direction to the left. In the case when obstacles are presented in both the left and right 
triangles, the vehicle avoids the nearest obstacle. The distance 1s computed two- 
dimensionally. The new orientation of the vehicle is 


D = Dyyrrent £ 0.257 (Eq. 4.3) 


desired 


where all the values are radians. 
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Obstacle A 


Obstacle B 


Vehicle 





Figure 4.4 Obstacle Avoidance Path 


C. FLOW OF BEHAVIORAL MOTION 


The behavioral motion control routine 1s shown in Figure 4.5. First, the vehicle 
determines the searching distance and checks the object list whether any objects are in the 
search area or not. When the vehicle detects an object in the left search triangle, it changes 
its direction to the right. When it detects any objects in right search triangle, it changes its 
path to the left to avoid the object. The desired direction is matched to the current vehicle 
orientation, then the vehicle resumes searching again. 

Motion is produced by the simplified simulation of dynamics presented in Chapter II 


and III. Object avoidance control must avoid the nearest imminent obstacle. This method 
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cause the vehicle to turn and collide with the neighbor obstacle, since we do not make path 


planning for behavioral motion. 






Sensing Object 








Determine Path 


Compute 
Orientation 
ompare 
Path 


Figure 4.5 Behavioral Motion Control Flow 
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V. IMPLEMENTATION ISSUES 


A. SYSTEM OVERVIEW 


Dynamic simulation has been implemented on Silicon Graphics IRIS graphics 
workstations. The units of the metric system that we use in this work are found in Table 5.1. 
For that reason, we convert those terms in the simulation then display in the Information 
Window as user friendly terms such as degrees and km/h. 

When the system 1s initialized, the main window displays the field of view from the 
vehicle as a default. Vehicle parameters and information are shown in the upper window as 
text. The flow of control of the system is illustrated in Figure 5.1. 

The approach to implementing motion dynamics is pre-processing control. In pre- 
processing control, two sequential steps are involved. First. appropriate forces and torques 
are found. All forces and torques are summed to produce a net force and torque acting on 


a vehicle, and these are used in the second step, dynamic analysis. The advantage of pre- 


processing 1s the dynamics control routines can be added and removed easily. 


Acceleration kgm /sec? kg m /sec? 


Speed Kilometers / hour meters /second 
Angle Degree Radian 
Mass tons kg 


Length meter 





Table 5.1 Parameter Unit Table 
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Set up 
Environment 
Vehicle Model 


Dynamics 
Computation 


Behavioral Interactive 
Control Control 


Figure 5.1 System Flow Overview 





B. VEHICLE PARAMETERS 


The 3D vehicle objects use text based NPSOFF file format [Zyda, 91]. Since NPSOFF 
is an application independent description of graphical objects, objects can be designed and 
maintained by general purpose tools [Zyda et.al., 92]. To explore the motion dynamics and 
behavioral simulation, the vehicle object data structure contains both graphics rendering 
and motion control variables shown as the following. We use structure type for vehicle 


parameters shown at Figure 5.2. Figure 5.2 shows variables and their descriptions. 
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float 


float 


float 


limit speed 
limit rpm 

max power 
min power 


pos[3] 
eye[3] 
lookatpt[3] 
lookfmpt[3] 
direction 
viewdirection 
wheel_angle 
elev 

gas 

rpm 

length 
height 

width 
cruise. speed 


vel[3] 
scalvel 
acc[3] 
scalacc 
force 
force 
rforce 
surge force 
drag force 
g force 
net torque 
torque 
roll 
pitch 

mass 


Figure 5.2 


maximum vehicle speed 
maximum engine rpm rate 
maximum engine power 
maximum brake power 


position coordinates in 3-D 

view position coodinates 

viewing coordinates 

viewing position coordinates 

vehicle direction 

viewing direction 

front wheel angle for wheeled vehicle 
elevation data 

remained gas amount 

throttle value 

vehicle length 

vehicle height 

vehicle width 

cruise speed when the cruise control 1s on 


velocity value x, y, z 

speed of the vehicle 

acceleration value x, y, z 
acceleration of the vehicle 

total external force to the vehicle 
left track force 

right track force 

total engine force of the vehicle 
total drag external forces to the vehicle 
external force by the gravity 

net torque to the vehicle 

total torque to the vehicle 

rolling moment 

pitchirfg moment 

net weight of the vehicle 


Ground Vehicle Structure 
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C. VEHICLE MOTION CONTROL 


1. Integration Of Forces 


The external forces and internal forces are integrated to find new velocity and 
orientation. All considered forces are defined in Chapter III. Depends on the user selection, 
the integrated forces are applied to the vehicle’s velocity and direction. All applied forces 
are defined in Chapter III. Figure 5.3 shows the algorithm of the summation of total forces. 
These forces result the accelerations using first-order equation f(t, p) described in Chapter 
II. Because of real-time limitations, we use only a single updated evaluation of fin each 


time step. 


2. Vehicle Speed And Direction 

The vehicle speed is computed by linear acceleration using Newton-Euler 
equations. Newton-Euler equation is a first order equation in the translational and angular 
velocities. To compute the vehicle position and direction, we use improved Newton-Euler 
equations defined in Chapter II. In kinematically based NPSNET, users can change the 
vehicle speed using SpaceBall or keyboard directly. And the vehicle speed is kept as a 
constant unless users do not change the vehicle speed. However in dynamic motion control, 
the vehicle speed is continuously changed by external forces. This makes the user use 
dynamic motion control difficult. Figure 5.4 shows the algorithm for vehicle’s acceleration 
and velocity. The maximum velocity is limited by the vehicle specification independently 
from the variance of forces. 

Figure 5.5 shows the algorithm for the computation of vehicle direction. As we 
mentioned in Chapter III, the vehicle’s direction is achieved by two methods, depending on 
vehicle type. Users can drive the vehicle using either the keyboard or SpaceBall. 

The integration of all forces and vehicle speed is shown in Figure 5.6. The new 
vehicle’s linear and angular velocity determine the next position and direction of the 


vehicle. This parameters render the next frame of the simulation. 
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integration() 

/* compute inertia tensor */ 

calc inertia(); /* compute inertia tensor of the vehicle */ 

angle momentum(); /* compute maximum roll and pitch */ 

sum linear forces(); /* compute surge force and drag force for the vehicle */ 
sum angular forces(); /* compute vehicle orientation forces */ 


if( behavior) /* user interactive control */ 
{ 
if('cruise flag) /* when cruise mode off *! 
{ 
/* compute linear and angular velocities */ 
calc_acceleration(); 
calc_velocity(); 
calc torque(); 
calc direction(,); 
) 
else /* when cruise mode on "| 
{ 
cruise on(); /* cruise speed computation */ 
velocity = cruise speed; 
calc torque(); 


calc direction(); 


) 
else /* behavioral control */ 
/* compute all the internal and external forces */ 
cruise on(); 
velocity = cruise speed; 
behavior motion(); 
calc inverse direction(); 
calc torque(); 
calc direction(); 


Figure 5.3 Algorithm For Integration Of All Forces 
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/* compute the linear acceleration of the vehicle */ 























calc_acceleration() 
/* compute acceleration by the Newton's second law F = ma */ 


acceleration = force I mass; 





/* divide the acceleration to x axis and z axis */ 
acceleration[X] = acceleration * fcos(vehicle direction); 
acceleration[Z] = acceleration * fsin(vehicle direction); 


j 


/* compute the linear velocity of the vehicle */ 
calc_velocity() 

/* compute the speed from the acceleration in m/sec unit */ 
velocity += deltatime * acceleration; 


/* limit the max speed between minimum and the maximum vehicle speed */ 
if velocity «2 MINSPEED) velocity - MINSPEED; 
if(velocity >= MAXSPEED) velocity = MAXSPEED; 





velocity[ X] = velocity * fcos(vehicle direction); 
velocity[Z] — velocity * fsin(vehicle direction); 


} 


Figure 5.4 Algorithm For Vehicle Acceleration And Velocity 
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/* compute angular acceleration and angular velocity by Euler method w = w * dt */ 
calc direction() 


{ 


if(vehicle_type == TRACKED) 
/* omega = torque / inertia tensor */ 
rotation = (net torque / inertia[Y]); 
dt rot = rotation * deltatime; 


direction += dt rot; 


- 


else /* Wheeled vehicle */ 

{ 
/* compute the difference between the wheel angle and direction */ 
tangle = ftan(wheel_angle); 
rotation = fatan(0.5 * tangle); 


/* if there are difference between the left and right forces 
it should be added to the vehicle direction value */ 
if(left force !2 right force) 
{ 
delia rotation = (net torque ! inertia[Y]); 


rotation += delta rotation; 


/* wheeled vehicle cannot rotate without translation */ 
if(velocity != 0.0) 
{ 

di rot = rotation * deltatime; 


direction += dt rot; 


Figure 5.5 Algorithm For Vehicle Direction Computation 
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Figure 5.6 Motion Control Flow 
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D. BEHAVIORAL CONTROL 


An autonomous vehicle requires perception, planning and control to act intelligently. 
Behavioral control is designed to explore use of dynamics both for interactive simulation 
and automatically controlled simulation. The goal of behavioral control in this simulation 
is to build a vehicle that can drive autonomously in the cultural and non-cultural objects 
environment. The behavior rule is avoidance of all obstacles. 

The method of obstacle avoidance in this simulation is simple. Figure 5.7 shows the 
process for obstacle avoidance. A vehicle's behavioral motion 1s assembied in sensing, 
planning and control. Function details are attached in Appendix B. The flow of behavioral 
motion 1s 

Sensing: Obstacle detection, Figure 4.2 

Planning: Path determination, Figure 4.3 


Control: Inverse dynamics computation for vehicle movement 


behavior motion() 
1 
search distance = fabs(velocity * 3.6); 
detect bounds(); /“ determine seraching boundary */ 


search object(); /* obstacle detection */ 


if(detect) /* path determination */ 


{ 
if(whichside == LEFT) 


dir value += FINE PI; /* turn the vehicle right */ 
else /* the object is on the right side */ 
dir value -= FINE PI; /* turn the vehicle left */ 


) 


else /* no avoidable objects */ 
dir value = 0.0; 


inverse dynamics(); /* compute inverse dynamics for vehicle motion */ 





Figure 5.7 Algorithm For Obstacle Avoidance 
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E. USER INTERFACES AND PERFORMANCE 


User interfaces have been developed to show the current status and vehicle 
information. The upper window in NPSNET displays current status of the system. The 
Main Window displays the view from the vehicle as a default. 

The vehicle dynamics model needs to know what the user is doing. Data acquisition 
is performed on the steering, brake, and other controls that are available to maneuver the 
vehicle. User input devices are shown in Appendix A. 

This simulation is done in real-time. The performance of NPSNET is not affected by 
dynamics. Performance of the current simulation system is approximately ten frames per 
second without texturing, and six frames per second with texturing. We find the effects of 


dynamic motion control are more realistic than kinematics-based control on bumpy terrain. 
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IV. CONCLUSIONS AND FURTHER WORK 


Current. graphics workstations support visual realism and high-performance 
computing. In this work, we investigate motion accuracy based on the physical laws to 
improve the NPSNET vehicle simulation. We have used the dynamics of rigid bodies and 
Euler equations for solving their equations of motion and a behavioral motion based on 
these dynamics. 

The simulation methods presented in this paper are implemented and run in real-time. 
Constraint dynamics results in reasonable vehicle motion as we expected. The application 
of dynamics for high-level motion control is working correctly and we can certify dynamics 
for low-level motion control in the next extension of the simulation. 

At present, we assume that vehicles are simple block structures to decrease the 
computational complexity and the Euler equation is selected for the same reason. We have 
not thoroughly explored all the possibilities for supporting the vehicle model. We do not 
consider complex natural parameters for this computation such as surface characteristics of 
the ground, tip-over forces and rebounding forces. For avoidnace motion, since vehicles do 
not save their past paths, current behavioral motion is not quite intelligent to select optimal 
path. 

This work suggests a number of topics for further research. Dynamics can be used for 
the low-level motion control and this can provide for many high-level control 1ssues. The 
vehicle simulation should handle multi-body vehicles. The higher performance of the 
graphics workstations and dynamics can lead the vehicle simulation realistic. As hardware 
performance improves, the vehicle dynamics can take advantage of the faster throughput. 


And more intelligent behavioral motion is a future avenue of work. 
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APPENDIX A 


USER INPUT DEVICES LAYOUT 


Dial Box 
Button Box 


Gun Elevation View Direction 


View Tilt 


; 
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Direction Z Dial 
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Space Ball Buttons 
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View Gun Elev View Tilt 
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Elevation 
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APPENDIX B 


Source Code For Dynamic Motion Control 


ү” 
* Dynamic motion control by the physics laws 
* Use Eulerian method for the velocities and positions 


Si 


tinclude “vehsim.h” 


Hinclude "vehcle.h” 


/* External dynamics forces variables in static */ 
float friction; /* Friction between the vehicle and the terrain */ 
float air_resist; /* Air resistance to the vehicle */ 


/* compute the dynamics of the vehicle the tilt and roll */ 
angle momentum(vehobj) 
struct vehicle * vehobj; 
| 
float fp(3],sp[3]; 


float xpos,ypos,zpos; /* x,y,z position of center of mass */ 


/* The pitch and roll are computed by taking a point 2 meters 
in front to get the pitch and 2 meters to the right sideto get the roll */ 


frontpoint[X] = fp[X] = vehobj->pos[X] + fcos(vehobj->direction) * 2.0; 
frontpoint[Z] = fp[Z] > vehobj->pos[Z] + fsin(vehobj->direction) * 2.0; 
frontpoint[Y] 2 fp[Y] = gnd_level(fp[X],fp{Z}); 


sidepoint[X] = sp[X) = vehobj->pos[X] + fcos(vehobj->direction+HALFPI) * 2.0; 
sidepoint[Z] = sp[Z] = vehobj->pos[Z] + fsin(vehobj->direction+HALFPI) * 2.0; 
sidepoint[ Y] 2 sp[ Y] = gnd_level(sp[X],sp[Z]); 


/* get the Y position of the vehicle */ 
xpos = vehobj->pos[X]; 

zpos = vehobj->pos[Z]; 

ypos = gnd level(xpos,zpos); 
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/* positive pitch is when the nose goes up */ 
vehobj->pitch = fatan((fp[ Y] - ypos)/2.0) / DEGTORAD; 
if (vehobj->pitch < 0.0) vehobj->pitch += 360.0; 


/* positive roll is when the right side goes down */ 
vehobj->roll = fatan((ypos - sp[Y])/2.0) / DEGTORAD; 
if (vehobj->roll < 0.0) vehobj->roll += 360.0; 


je 

/* compute gravity force on an inclined surface 
vi 

calc gforce(vehobj) 

struct vehicle *vehobj; 


float pitch angle; 
float rad angle; 


pitch. angle = vehobj->pitch; 


/* compute the sliding value from the pitching angle of the vehicle */ 

if(pitch, angle == 0.0) ( 
rad angle = 0.0; 

) 

/* sliding force is the SIN value of the grade */ 

else 

| 
if(pitch angle > 0.0 && pitch angle < 90.0) /* the vehicle nose up */ 
rad angle = fsin(pitch angle * DEGTORAD); 


if(pitch angle < 360.0 kk pitch. angle > 270.0) /* the vehicle nose down*/ 
rad angle = fsin((360.0 - pitch angle) * DEGTORAD); 


/* f = mg x sinO in case of g=9.81 and ground elevation */ 
if(vehobj->scalvel != 0.0) 
| 


if(vehobj->scalvel > 0.0) /* if the vehicle moves forward */ 


| 
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if(pitch angle > 0.0 && pitch angle < 90.0) 
vehobj->g force = (vehobj->mass * GRAVITY * rad angle); 
else 
vehobj->g force = -(vehobj->mass * GRAVITY * rad angle); 
) 
else /* the vehicle moves backward */ 
( 
if(pitch angle > 0.0 && pitch. angle < 90.0) 
vehobj->g force = -(vehobj-»mass * GRAVITY * rad angle); 
else 
vehobj->g force = (vehobj->mass * GRAVITY * rad angle); 


| 
else /* the vehicle speed = 0 */ 
{ 
if(vehobj->pitch > 0.0 & & vehobj->pitch < 90.0) 
vehobj->g_force = -(vehobj->mass * GRAVITY * rad angle); 
else 
vehobj->g_force = (vehobj->mass * GRAVITY > rad, angle); 
| 


mend ¢ force */ 


/* compute the drag forces to the vehicle, friction specifically 


calc drag force(vehobj) 


struct vehicle * vehobj: 


float vel; 


/* compute the air resistance to the vehicle */ 
/* approximate the value to the function of square of velocity */ 
if(vehobj->scalvel > 0.0) 
air_resist = vehobj->scalvel * vehobj->scalvel * 10; 
else 
| 
if(vehobj->scalvel != 0.0) 
air resist 2 -(vehobj-»scalvel * vehobj-»scalvel * 10); 
else /* the speed is 0 */ 
air resist = 0.0; 
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/* compute the fnction between the ground and the vehicle */ 
/* approximation coefficient between the velocity and friction */ 
vel = fabs(vehobj->scalvel); 


if('SIDE. BREAK) 
( 
if(vehobj->scalvel == 0.0 && vehobj->scalacc == 0.0) 
/* The vehicle ts stalled */ 
friction = 0.0; 
else if(vehobj->scalvel == 0.0 && vehobj->scalacc != 0.0) 
/* static friction */ 
fricuon = SFRICTION + 1000.0 + (vehobj->mass); 


else if(vehobj->scalvel > 0.0 && vehobj->scalacc != 0.0) 
/* dynamic friction */ 
fricuon = DFRICTION + (vehobj->mass); 


else if(vehobj->scalvel < 0.0 && vehobj->scalacc != 0.0) 
fricuon = -(DFRICTION + vehobj->mass); 


vehobj->drag force = -(friction + air resist); 


) /* emd drag. dorce */ 


je 
/* compute the engine force to the vehicle and breaking force 

ER 

calc engine force(vehobj) 

struct vehicle * vehobj; 

{ 

if(Stop_Engine == FALSE) 

{ 

/* compute the linear translation surge force by the summation of the left and right forces */ 
vehobj->surge_force = vehobj->rforce + vehobj->lforce; 
if(vehobj->scalvel > 0.0) 

{ 
if(vehobj->surge_force >= MAXPOWER) 
vehobj->surge_force = MAXPOWER; 
if(vehobj->surge_force <= MINBREAK) 
vehobj->surge force = MINBREAK; 
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) 

if(vehobj->scalvel < 0.0) 

{ 
if(vehobj->surge force <= MINPOWER) vehobj->surge force = MINPOWER; 
if(vehobj->surge, force >= MAXBREAK) vehobj->surge. force = MAXBREAK; 


else 


vehobj->surge force = 0.0; 
vehobj->torgue = 0.0; 
) 


) /* end engine force */ 


eg 

/* compute inertia tensor for each axis 

"n 

calc inertia(vehobj,nertia,inertia matrix) 

struct vehicle * vehobj; 

float inertia[6]; 

float inertia. matrix [3][3]; 

| 
/* assume that the vehicle is block wih the same mass density */ 
/* principal axes inerua */ 
inertia[ X] = (vehobj->mass * (vehobj->length * vehobj->height)) / 12.0; 
inertia[Y] 2 (vehobj->mass * (vehobj->width * vehobj->length)) / 12.0; 
inertia[Z] = (vehobj->mass * (vehobj->width * vehobj->height)) / 12.0; 


/* secondary axes inertia in block structure */ 

inertia[ XY] = (vehobj->mass * (vehobj->width * vehobj->height)) / 2.0; 
inertia[ XZ] = (vehobj->mass * (vehobj->width * vehobj->length)) / 2.0; 
inertia[ YZ] = (vehobj->mass * (vehobj->length * vehobj->height)) / 2.0; 


* make an inertia tensor matrix */ 
inertla_matrix[X][X] = inertia[X]; 
inertia matrix[X][Y] = -inertia[ XY]: 
inertia matrix[X][Z] = -inertia[ XZ]; 
inertia matrix[Y][X] = - inertia[ XY]; 
inertia matrix[ Y][Y] = inertia[Y]; 
inertia_matrix[Y][Z] = inertia[ YZ]; 
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inertia matrix[Z][X] 2 -1nertia[ XZ]; 
inertia matrix[Z][Y] 2 -inertia( YZ]; 
inertia. matrix [Z][Z] 2 inertia[Z ]; 


) /* end calc. inertia */ 


0 

* bis the determinant of 3x3 matrix a 

7 

det(a,b) 

float a[3][3]; 

float *b; 

{ 

*b=a[X][X] * (Q[Y]CY] * a[Z] [2] - a[Y][Z] * a[Z][YD 

+a[X][Y] * (a[Y][Z] * a[Z][X] - a[Y][X] * a[Z](Z]) 
+ a[XJ[Z] * (aL YJ[X] * a[Z][Y] - a(YJ[Y] * a[Z][X]): 

| 

y^ 

* compute inverse matrixof 3X3 inertia matrix 

AR 


calc inverse inertia(inertia matrix,inv inertia matrix) 
float inertia matrix[3][3]; 
float inv inertia matrix[3][3]; 
( 
float d; 
det(inertia matrix,&d); 


inv inertia matrix[X][X] = (inertia matrix[1][1] ” inertia matrix[2][2] 


- inertia. matrix[1][2] * inertia. matrix[2][1])/d; 


inv inertia matrix[Y][X] = (inertia matrix[Y][Z] * inertia_matnx[Z][X] 


- inertia matrix[Y][X] * inertia matrix[Z][Z])/d; 


inv inertia matrix[Z][X] = (inertia matrix[Y)[X] * inertia matrix[Z][Y ] 


- inertia matrix[Y][Y] * inertia matrix[Z][ X])/d; 


inv inertia matrix[X][Y] 2 (inertia matrix[Z][Y] * inertia matrix[X][Z] 


- inertia matrix[Z][Z] * inertia matrix[X][Y ])/d; 


inv inerua, matrix[Y][Y] 2 (inertia matrix[Z][Z] * inertia matrix(X][X] 


- inertia. matrix[Z][X] * inertia matrix[X][Z])/d; 


inv inertia matrix[Z][Y] - (inertia, matrix(Z][X] * inertia matrix( X][Y] 


- inertia. matrix[Z][Y] * inertia matrix[X][ X])/d; 


inv. inertia matrix[X][Z] 2 (inertia matrix[X][Y] * inertia matrix[Y][Z] 
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- Inertia_matrix[X][Z] * inertia_matrix[Y][Y])/d; 

inv inertia matrix[Y][Z] = (inertia matrix[X][Z] * inertia matrix(Y][X] 
- inertia matrix[X][X] * inertia_matrix[Y][Z])/d; 

inv inertia matrix[Z][Z] = (inertia_matrix[X][X] * inertia_matrix[Y][Y] 
- inertia_matrix[X][Y] * inertia_matrix[Y][X])/d; 


[* 
* compute torque value for rotation 
ai 
calc. torque(vehobj) 
struct vehicle *vehobj; 
( 
if(vehobj->lforce == vehobj->rforce) 
vehobj->net_torque = 0.0; 
else 
| 
if(vehobj->scalvel == 0.0) 
| 


vehobj->net torque = vehobj->torque * 0.6; 


vehobj->net_torque = vehobj->torque * 0.8; 


if(vehobj->type == WHEELED && vehobj->scalvel == 0.0) 
vehobj->net_torque = 0.0; 


/* 
* compute the total linear forces to the vehicle 
sum linear forces(vehobj, deltaume) 

struct vehicle *vehobj; 

float deltatime; 


| 


struct vehicle tvehpos; 


tvehpos = *vehobj; 


47 


if(Stop Engine == FALSE) 

( 
calc drag force(&tvehpos); /* compute friction and air resistance */ 
calc_gforce(&tvehpos); /* compute gravity force */ 
calc_engine_force(&tvehpos); /* compute engine force */ 


/* compute the total forces 
each forces are scalar values, so those values can be summed linearly */ 
tvehpos.force = tvehpos.drag_force+tvehpos.surge_force+tvehpos.g_force; 


if(tvehpos.force > MAXFORCE) tvehpos.force = MAXFORCE; 
if(tvehpos.force < MINFORCE) tvehpos.force = MINFORCE; 


*vehobj = tvehpos; 


E 

/* compute the total angular forces to the vehicle 
*/ 

sum, angular, forces(vehobj) 

struct vehicle *vehobj; 


| 


float abstorque; 


/* compute the angular force to the vehicle by the right hand rule */ 
vehobj->torque = vehobj->lforce - vehobj-»rforce; 


/* absolute value of torque */ 
abstorque = fabs(vehobj->torque); 


/* for the smooth mouon for the wheeled vehicle */ 

if(vehobj->type == WHEELED && abstorque <= 100.0) 
vehobj->torgue = 0.0; 

if(vehobj->torgue >= MAXPOWER) vehobj->torgue = MAXPOWER; 


if(vehobj->torgue <= MINPOWER) vehobj->torgue = MINPOWER; 


Kë 


* compute the linear acceleration of the vehicle 
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ни 

calc_acceleration(vehob}) 

struct vehicle * vehobj; 

{ 
/* compute acceleration by the Newton’s second law F = ma */ 
vehobj->scalacc = vehobj->force / vehobj->mass; 


/* divide the accleration to x axis and z axis */ 
vehobj->acc[X] = vehobj->scalacc * fcos(vehobj->direction); 
vehobj->acc[Z] = vehobj->scalacc * fsin(vehobj->direction); 


0 

* compute the linear velocity of the vehicle 

E) 

calc, velocity(vehobj,deltatime) 

struct vehicle * vehobj; 

float deltatime: 

( 
/* compute the speed from the acceleration in m/sec unit */ 
vehobj->scalvel += deltatime * vehobj->scalacc; 


/* to avoid the numerical instability near 0 speed */ 
if(vehobj->scalvel <= 0.3 && vehobj->scalvel >=-0.3) 
vehobj->scalvel = 0.0; 


/* limit the max speed between minimum and the maximum vehicle speed */ 
if(vehobj->scalvel <= MINSPEED) vehobj->scalvel = MINSPEED; 
if(vehobj->scalvel >= MAXSPEED) vehobj->scalvel = MAXSPEED; 


vehobj->vel[X] = vehobj->scalvel * fcos(vehobj->direction); 
vehobj->vel[Z] = vehobj->scalvel * fsin(vehobj->direction); 


is 

* compute angular acceleration and angular velocity by Euler method w = w * dt 
v) 

calc direction(vehobj,inertia,deltatime) 

struct vehicle * vehobj; 

float inertia[6]; 

float deltatime; 


49 


float rotation; /* rotation value by the torque */ 

float dt rot; /* rotation times deltatime */ 

float ext_rot; /* another rotation value for wheeled vehicle */ 
float tangle; /* tangent angle for wheeled vehicle */ 


if(vehobj->type == TRACKED) 

| 
/* omega = torque / inertia tensor */ 
rotation = (vehobj->net_torque / inertia[Y]); 
dt_rot = rotation * deltatime; 


if(dt_rot < 0.0) 

dt_rot = dt_rot + DPI; 
else if(dt_rot > DPI) 

dt_rot = dt rot - DPI, 


vehobj->direction += dt rot; 

if(vehobj->direction < 0) 
vehobj->direction += DPI; 

else if (vehobj->direction > DPI) 
vehobj->direction -= DPI; 


else /* Wheeled vehicle */ 

| 

/* compute the difference between the wheel angle and direction */ 
tangle = ftan(vehobj-» wheel angle); 

rotation = fatan(0.5 * tangle); 


/* if there are difference between the left and right forces 
it Should be added to the vehicle direction value */ 
if(vehobj->Iforce != vehobj->rforce) 
{ 
ext_rot = (vehobj-»net torque / inertia[ Y ]); 
rotation 4- ext rot; 


/* wheeled vehicle cannot rotate without translation */ 
if(vehobj->scalvel != 0.0) 


| 
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dt rot = rotation * deltatime; 


if(dt rot < 0.0) 

dt rot = dt rot + DPI; 
else if(dt rot > DPI) 

dt rot = dt rot - DPI; 


vehobj->direction += dt rot; 


if(vehobj->direction < 0) 
vehobj->direction += DPI; 

else if (vehobj->direction > DPI) 
vehobj->direction -= DPI; 


Tt 
* compute the wheel angle to direct to the desired direction 
*/ 
calc inverse direction(vehobj,des direction,deltatime) 
struct vehicle *vehobj: 
float *des direction; 
float deltaume; 
{ 
float rad direction; 
float del angle; 


rad direction = *des direction; 
if(vehobj->type == WHEELED) 
( 
if(vehobj->direction != rad. direction) 
( 
del angle = fatan(2.0 * ftan(rad_direction)); 
vehobj->wheel angle = deltatime * del angle; 


if(vehobj->wheel angle >= 45.0) 
vehobj->wheel angle = 45.0; 

if(vehobj->wheel angle <= 45.0) 
vehobj->wheel_angle = -45.0; 
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else 


rad direction = 0.0; 


) 
else /* tracked vehicle */ 


( 
if(vehobj->direction != rad direction) 
( 
if(rad direction > 0.0) 
| 


vehobj->lforce += (rad direction * 10.0) * (vehobj->mass /1000.0); 


vehobj->rforce -= (rad direction * 10.0) * (vehobj->mass /1000.0); 


| 


else /* desired direction 1s less than O */ 


| 


vehobj->lforce -= (rad direction * 10.0) * (vehobj->mass /1000.0); 
vehobj->rforce += (rad direction * 10.0) * (vehobj->mass /1000.0) 


else 
rad direction = 0.0; 


*des direction = rad direction; 


ја 

* integrate all forces 

integration(vehobj,cruise,behavior,deltatime) 

struct vehicle *vehobj; 

int *CTUISE; 

int *behavior; 

float deltatime; 

( 
struct vehicle tvehpos; 
float inertia body[6]; 
float path. direction; 
float inertia matrix (3](3]; 
int cruise, flag; 
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? 


tvehpos = *vehobj; 
path direction = tvehpos.direction; /* current vehicle direction */ 
cruise flag = *cruise; 


if(!*behavior) 
{ 
if(SIDE_BREAK == FALSE) 
| 
/* compute inertia tensor */ 
calc inertia(&tvehpos,inertia body,inertia matrix); 
angle momentum(&tvehpos); /* compute roll and pitch */ 


if('cruise. flag) 

{ 
/* compute all the internal and external forces */ 
sum_linear_forces(&tvehpos,deltatime); 
sum_angular_forces(&tvehpos); 


/* compute linear and angular velocities */ 
calc_acceleration(&tvehpos); 
calc_velocity(&tvehpos,deltatime); 
calc_torque(&tvehpos); 
calc_direction(&tvehpos,inertia_ body ,deltatime); 

) 

else / cruise on */ 

{ 
sum_linear_forces(&tvehpos,deltatime); 
sum_angular_forces(&tvehpos); 
cruise_on(&tvehpos); 


tvehpos.scalvel = tvehpos.cruise_speed; 
tvehpos.vel[X] = tvehpos.scalvel * fcos(tvehpos.direction); 
tvehpos.vel[Z] = tvehpos.scalvel * fsin(tvehpos.direction); 


calc torque(&tvehpos); 
calc_direction(&tvehpos,inertia_body,deltatime); 


) 


else /* when the side break is locked */ 
{ 
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} 


tvehpos.rforce = 0.0; 
(уеһров.Шогсе = 0.0; 
tvehpos.g. force = 0.0; 
tvehpos.drag force = 0.0; 
tvehpos.scalvel = 0.0; 


else /* behavioral function */ 


| 


/* compute inertia tensor */ 
calc inertia(&tvehpos,inertia body,inertia matrix); 
angle momentum(&tvehpos); /* compute roll and pitch */ 


/* compute all the internal and external forces */ 
sum linear. forces(&tvehpos,deltatime); 

sum, angular. forces( &tvehpos); 

cruise on(&tvehpos); 


tvehpos.scalvel = tvehpos.cruise speed; 
tvehpos.vel[X] = tvehpos.scalvel * fcos(tvehpos.direction); 
tvehpos.vel[Z] 2 tvehpos.scalvel * fsin(tvehpos.direcuon); 


behavior mouon(&tvehpos,&path direction); 
calc inverse direction(&tvehpos,&path direction,deltatime); 
calc torque(&tvehpos); 

calc direction(&tvehpos,inertia body,deltatime); 


/* inverse dynamics for the vehicle direction */ 


/* compute miscelaneous factors of the vehicle */ 
calc_gas(&tvehpos,deltaume); 


calc_rpm(&tvehpos); 


*vehobj = tvehpos; 


* compute the new x and z position by the linear and angular velocity 


new. position(vehobj,cruise,behavior,deltatime) 
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struct vehicle * vehobj; 


int *cruise; 
int *behavior; 
float deltatime: 


| 


Struct vehicle tvehpos; 
int behavior flag; 
int cruise flag; 


tvehpos = *vehobj; 
behavior. flag = *behavior; 
cruise flag = *cruise; 


/* compute acceleration and velocity */ 
integration(&tvehpos,&cruise flag,&behavior flag,deltatime); 


/* Euler Intergration, Pn = Pn-1 + (Vt * dT) + (ACC * dT * dT / 2) */ 

tvehpos.pos[X] += (tvehpos.vel[ X] * deltatime) + (tvehpos.acc[X] * deltatime * deltaume / 2.0); 
tvehpos.pos[Z] += (tvehpos.vel[Z] * deltatime) + (tvehpos.acc[Z] * deltatime * deltatime / 2.0); 
tvehpos.pos[Y] = gnd_level(tvehpos.pos[X],tvehpos.pos[Z]); 


*vehobj = tvehpos: 
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APPENDIX C 


Source Code For Behavioral Motion 


es 
* determine the search tri and find any objects in there 
Så 


Hinclude "vehsim.h” 
#include “‘vehcle.h” 


include “externs.h” 


/* Need a variable to hold the radius of the vehicles’ turning that appropriate valuc to avoid the object */ 
float RADIUS = 5.0; 


int distbetween2(); /* This function finds the distance between 2 points */ 
int seq. search(); /* search obstacle table array */ 


A 
* detect non-moving obstacles 
и 
detect nonmoving objects(vehobj) 
struct vehicle *vehobj; /* actual object being checked for avoidance */ 
| 
float radius of vehicle and object; 
int xgrid, zgrid; /* gridsquare indices */ 


xgrid = (int)(vehobj->pos[X ]/GRIDSIZE); 
zgrid = (int)(vehobj->pos[Z]/GRIDSIZE); 


Få 

*this function constructs the search triangle given the at look at point 
SA 

detect bounds(vehobj,vehdir,searchtri,advance) 

struct vehicle *vehobj: 

float * vehdir; 

float searchtri[4][3]; 
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float advance; 


float segdir; 
float langle, rangle; 


segdir = *vehdir; 


/* determine the angles of the view bounds */ 
rangle = segdir - HFOS; 
langle = segdir + HFOS; 


if(rangle < 0.0) rangle = DPI + rangle; 
if(langle > DPI) langle = langle - DPI; 


/* compute the center vertex of the triagnle */ 
searchtri[2][X] = vehobj->pos[X]; 
searchtri[2][Z] = vehobj->pos[Z]; 


/* compute the left vertex of the triagnle */ 
searchtri(0][X] = searchtri(2][X] + fcos(langle) * (SEARCHOFFSET+advance); 
searchtri(0)[Z] = searchtri(2][(Z] + fsin(langle) * (SEARCHOFFSET+advance); 


/* compute the right vertex of the triagnle */ 
searchtri(1][X] 2 searchtri[2][X] + fcos(rangle) * (SEARCHOFFSET+advance); 
searchtri(1][Z] = searchtri[2](Z] + fsin(rangle) * (SEARCHOFFSET +advance); 


/* compute the center vertex between the left and right vertices */ 
searchtri[3][X] = (searchtri[O][X] + searchtn{1][X]) / 2.0; 
searchtri[3][Z] 2 (searchtri(0][Z] 4 searchtri[1][Z]) / 2.0; 


Мы 

* serach obstacle array for avoidance 

E 

search object(searchtri find. object,lrvalue,vehicle dir) 
float searchtri[4 ][3]; 

int *find object; 

int *Irvalue; 

float vehicle_dir; 


| 


ST 


float leftminx,nighuninx,leftminz,rightminz; 
float leftmaxx,nghtmaxx,leftmaxz,nghtmaxz; 
float lobjx, lobjz, robjx, robjz; 

int left search,right search,overlapped; 

int intdir; 

float inverse, dir, tempx, tempz; 

int Idistance,rdistance; 


/* reinitialize the flags */ 
*find object = FALSE; 
left. search = FALSE; 
right, search = FALSE; 


boundary(&searchtri[2][X],&searchtri[0][ X],&searchtri[3]( X],&leftminx ,&leftmaxx); 
boundary(&searchtri[2][ X ],&searchtri(1][ X], &searchtri[3][ X],&rightminx,&rightmaxx); 
boundary(&searchtri[2][Z],&searchtri[O][Z], & searchtri[3][Z],&leftminz,& leftmaxz); 
boundary(&searchtri[2][Z],& searchtri[ 1] [Z],&searchtri[3][Z],&rightminz &rightmaxz); 


left search 2 seq search(&leftminx,&leftmaxx,&leftminz,&leftmaxz,&lobjx ,&lobjz); 
right search 2 seq. search(&rightminx,&rightmaxx ,&rightminz,& righumnaxz,&robjx ,&robjz); 


/* check if the object is in between the left tri and right їп */ 
if((lobjx 22 robjx) && (lobjz 2- robjz)) f 
overlapped 2 TRUE; 
} 
else ( 
overlapped = FALSE; 


if(vehicle dir « 0.0) 
vehicle dir += DPI; 


if(vehicle dir == 0.0 || vehicle dir 22 360.0) 
intdir = 1; 

else if(vehicle dir > 0.0 && vehicle dir < ОТЕ РІ) 
intdir = 2; 

else if(vehicle dir == QTR PD 
intdir = 3; 

else if(vehicle_dir > QTR PI && vehicle dir « HALFPI) 
intdir = 4; 

else if(vehicle dir == HALFPI) 
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intdir = 5; 

else if(vehicle dir > HALFPI && vehicle dir < THREE QTR PI) 
intdir = 6; 

else if(vehicle dir == THREE QTR PI) 
intdir = 7; 

else if(vehicle dir > THREE QTR PI && vehicle dir < DPI) 
intdir = 8; 


if(overlapped) 


{ 


switch(intdir) 


{ 


case 1: /* direction = 0 or 2pi */ 
if(searchtri[2][ X] >= lobjx) 
*Irvalue = LEFT; 
else 
*Irvalue = RIGHT; 
break; 


case 2: /* 0 < direction < 0.5p1 */ 

inverse_dir = QTR_PI - vehicle_dir; 
tempx = fabs(lobjx - searchtri(2)(X])); 
tempz = ftan(inverse dir) * tempx; 
tempz = searchtri[2] [Z] - tempz; 
if(lobjz >= tempz) 

*Irvalue = RIGHT; 
else 

*Irvalue = LEFT; 
break; 


case 3: /* direction = 0.5p1*/ 
if(searchtri(2][Z] >= lobjz) 
*Irvalue = LEFT; 
else 
*Irvalue = RIGHT; 
break; 


case 4: /* 0.5pi < direction < pi */ 
inverse dir = vehicle dir; 
tempz = fabs(lobjz - searchtri[2][Z]); 
tempx = fabs(ftan(inverse dir) * tempz); 
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tempx = searchtn[2][X] + tempx; 
if(lobjx >= tempx) 

value = LEFT, 
else 

*Irvalue = RIGHT; 
break; 


case 5: /* direction = pi */ 
if(searchtri[2][X] <= lobjx) 
*Irvalue = LEFT; 
else 
*|rvalue 2 RIGHT; 
break; 


case 6: /* pi < direction < 1.5pi */ 

inverse dir = vehicle dir - РІ; 
tempz = fabs(lobjz - searchtri[2][Z]); 
tempx - fabs(ftan(inverse dir) * tempz); 
tempx = searchtri[2][X] - tempx; 
if(lobjx >= tempx) 

*Irvalue = LEFT; 
else 

*|rvalue = RIGHT; 


break; 


case 7: /* direction = 1.5pi*/ 
if(searchtri[2][Z] <= lobjz) 
*Irvalue = LEFT; 
else 
*Irvalue = RIGHT; 
break; 


case 8: /* 1.5pi < direction <= 2pi */ 
inverse dir = vehicle dir - THREE QTR PI, 
tempx = fabs(lobjx - searchtri[2][X]); 
tempz = fabs(ftan(inverse_dir) * tempx); 
tempz = searchtri[2][Z] - tempz; 
if(lobjz >= tempz) 
*Irvalue = LEFT; 
else 
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*Irvalue = RIGHT: 
break; 


*find object = TRUE; 


else /* the object is in left tri or right tri */ 

{ 

if(left search && right search) 

{ 
Idistance = dist. in, 2d(lobjx, lobjz, searchtri[2][X], searchtri[2][Z]): 
rdistance = dist, in 2d(robjx, robjz, searchtri[2][ X], searchtri[2][Z]); 


if(Idistance >= rdistance) 
*Irvalue = LEFT; 
else 
*|rvalue 2 RIGHT; 


else /* among right, left or none object */ 


{ 
if(left_search) 
{ 
*find_object = TRUE; 
*irvalue = LEFT; 


else 


if(right search) 

{ 
*find object = TRUE; 
*Irvalue = RIGHT; 


else /* !left search && tright search */ 


{ 
*find object = FALSE; 
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} 7 atheoulemerse 4) 


seq_search(minx ,maxx ,minz,maxz,Objx,objz) 
float *minx, *maxx, *minz, *maxz, *objx, *objz; 
int ix; 


float tempobjx, tempobjz; 


for(ix=0;1x<=100;ix++) 

| 
tempobjx = avoidlist[ix].center[X]; 
tempobjz = avoidlist[ix].center[Z]; 


if(tempobjx >= *minx && tempobjx <= *maxx && 
tempobjz >= *minz && tempobjz <= *maxz) 


*objx = tempobjx; 
*objz = tempobjz; 
return( TRUE); 


} 
return(FALSE); 
j 
ЦЕ 
* determine the minimum and maximum number among the three floating point numbers 
2 
boundary(x 1,x2,x3,minno,maxno) 
float "xl x3 
float *minno,*maxno; 
| 


float 1x,}x,kx; 


х= M 
jx = * x2: 
kx z *x3; 
*minno = 1х; 


*MAXNO =I 
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if (jx < *minno) *minno = jx; 
if Gx > *maxno) *maxno = jx; 
if (kx < *minno) *minno = kx; 
if (kx > *maxno) *maxno = kx; 


/*this function computes the 3D distance between two points, it returns an interger*/ 
dist in 2d(subjectx,subjectz,objectx,objectz) 
float subjectx,subjectz,objectx,objectz; 
( 
float distx,distz; 
int dist; 


distx 2 subjectx - objectx; 
distx = distx % distx; 

distz = subjectz - objectz; 
distz = distz > distz; 

dist = (int)fsqrt((distx+distz)); 
return(dist); 
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